/******************************************************************************
* AIR SYSTEMS LABORATORY
* AEROSPACE ENGINEERING
* UNIVERISTY OF TEXAS AT AUSTIN
* UAV FLIGHT SOFTWARE 
* DESCRIPTION:
*   
*
* SOURCE: 
* Orignial: 08/14/2008  Jorge Alvarez joroge@gmail.com
* Modified: 03/20/2009	Sergio García-Nieto sergarro@isa.upv.es
* $Id: idle_thread.c 4 2009-07-21 17:12:11Z joroge@gmail.com $
*************************************************************************************/
#include <time.h>
#include <fcntl.h>
#include <stdio.h>
#include <termios.h>

#include "utilities.h"
#include "globaldefs.h"
#include "adc.h"
#include "gps.h"
#include "imu.h"
#include "serial.h"

void *idle()
{
	//static struct gps *gpsPacketPtr=NULL,gpsPacket;
	struct timespec t0,t1,t0_ant,wait,next_activation,Period,delay;
	_uint64 auxtime;
	static struct imu *imuPacketPtr=NULL,imuPacket;
	static struct imuShort *imuShortPacketPtr=NULL,imuShortPacket;
	static struct gps *gpsPacketPtr=NULL,gpsPacket;
	static struct adc *adcPacketPtr=NULL,adcPacket;
	static struct gpsAdc *gpsAdcPacketPtr=NULL,gpsAdcPacket;
	
	int fd1,fd2,sPort1,sPort2,sPort3;
	int nbytes;
	long int counter;
	
	//Pointer Inicialization
	imuPacketPtr=&imuPacket;
	imuShortPacketPtr=&imuShortPacket;
	gpsPacketPtr=&gpsPacket;
	adcPacketPtr=&adcPacket;
	gpsAdcPacketPtr=&gpsAdcPacket;
	
	/*********************************************************************
	 *Open and configure USB Serial Port1 (GPS)
	 *********************************************************************/
	sPort1 = open_serial(SERUSB_PORT1,BAUDRATE_115200);
	/*********************************************************************
	 *Open and configure USB Serial Port2 (IMU)
	 *********************************************************************/
	sPort2 = open_serial(SERUSB_PORT2,BAUDRATE_115200);
	/*********************************************************************
	 *Open and configure Serial Por4 (ADC)
	 *********************************************************************/
	sPort3 = open_serial(SERIAL_PORT4,BAUDRATE_38400);
	
	// Request for Data
	gpsRequest(sPort1);
	imuRequest(sPort2);
	adcRequest(sPort3);
	
	//Delay Waiting for Data
	delay.tv_sec = 0;
	delay.tv_nsec = 200000000;
	
	clock_gettime(CLOCK_REALTIME, &t0_ant);
	printf ("Idle Task on-line.\n");
	while(1) 
	{   
		// Loop time claculations
		clock_gettime (CLOCK_REALTIME, &t0);
		next_activation=sum_tiempos(t0,Ts_idle);
		
		printf("IDLE Running \n");
			
		
		// Reading IMU Sensor
		imuAcquisition(sPort2,imuPacketPtr);

		// Reading GPS Sensor
		gpsAcquisition(sPort1,gpsPacketPtr);

		// Reading ADC Sensor
		adcAcquisition(sPort3,adcPacketPtr);
		
		// Clear Buffers
		tcflush(sPort1, TCIFLUSH);
		tcflush(sPort2, TCIFLUSH);
		tcflush(sPort3, TCIFLUSH);
		
		// Request for Data
		gpsRequest(sPort1);
		imuRequest(sPort2);
		adcRequest(sPort3);
		
		// Save IMU Data in FIFOs
		counter = counter + 1;
		imuShortPacket.pitch = imuPacket.pitch;
		imuShortPacket.roll = imuPacket.roll;
		imuShortPacket.yaw = imuPacket.yaw;
		imuShortPacket.Ts = ((int)Period.tv_nsec)/prec;
		imuShortPacket.counter = counter;

		// Telemetry FIFO
		fd1 = open(fifoAttiTele,O_WRONLY | O_NONBLOCK);
		if (fd1>0){
		
			nbytes = write(fd1,imuShortPacketPtr,sizeof(struct imuShort));
			//printf("[attitude FIFO write] %d \n",nbytes);
			close(fd1);
		}
		// Save Data in FIFOs
		counter = counter + 1;
		gpsAdcPacket.lat = gpsPacket.lat;
		gpsAdcPacket.lon = gpsPacket.lon;
		gpsAdcPacket.alt_msl = gpsPacket.alt_msl;
		gpsAdcPacket.ve = gpsPacket.ve;
		gpsAdcPacket.vn = gpsPacket.vn;
		gpsAdcPacket.vd = gpsPacket.vd;
		gpsAdcPacket.airSpeed = adcPacket.airSpeed;
		gpsAdcPacket.pAltitude = adcPacket.pAltitude;
		gpsAdcPacket.Ts = ((int)Period.tv_nsec)/prec;
		gpsAdcPacket.counter = counter;


		// Telemetry GPS FIFO
		fd2 = open(fifoNavTele,O_WRONLY | O_NONBLOCK);
		if (fd2>0){
			//printf("[Nav FIFO open] %d \n",fd);
			nbytes=write(fd2,gpsAdcPacketPtr,sizeof(struct gpsAdc));
			//printf("[Nav FIFO write] %d \n",nbytes);
			close(fd2);
		}

		// Print the results
		//if(v) printf("Inicialization--> WCCT: %3d Period: %3d\n",((int)WCCT.tv_nsec)/prec,  ((int)t0.tv_nsec-(int)t0_ant.tv_nsec)/prec);
		//if(v) printf("I %3d %3d \n",((int)WCCT.tv_nsec)/prec,  ((int)Period.tv_nsec)/prec);
		
		// Wait until the next activation and Calculate Period
		Period = diff_tiempos(ID_idle,t0,t0_ant,1);
		t0_ant = t0;
		clock_gettime (CLOCK_REALTIME, &t1);
		if (timespec2nsec(&next_activation)>timespec2nsec(&t1))
		{
		    auxtime = (timespec2nsec(&next_activation)-timespec2nsec(&t1));
			nsec2timespec(&wait,auxtime);
			nanosleep(&wait, NULL);
		}
	}
	//Close Ports
	close(sPort1);
	close(sPort2);
	close(sPort3);

	return(NULL);
		
}
